Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /* Program udarbejdet Af Andreas Christoffersen
- * v/AC Maskin Service ApS for Topsil 24-03-2018
- * REV. 3.09
- */
- #include <AccelStepper.h>
- //Definer I/O adresser
- #define pin_Step1 8 //FREM
- #define pin_Step2 9 //TILBAGE
- #define pin_Stop 2 //STOP
- #define pin_Kal 3 //START KALIBRERING
- #define pin_UdgPos 4 //UDGANGS POSITION
- #define pin_DIR 5 //RETNING
- #define pin_STEP 6 //STEP PULS
- #define pin_EN 7 //ENABLE
- #define pin_Kvit 10 //KVITTERING TIL PLC
- //Definer konstanter
- #define ACCELERATE_Q_RUN 200 //acceleration kvotient udner drift
- #define ACCELERATE_Q_KAL 200 //acceleration kvotient under kalibrering
- #define MAX_SPEED 300 //Højeste hastighed for stepper
- #define MAX_SPEED_KAL 300 //Højeste hastighed for kalibrering
- #define Afstand 1600 //Afstand der skal køres
- #define kalAfstand 200 //Afstand kalibreringen kører
- #define debounceCount 5 //debounce tid
- //Definer variabler
- bool nyStart = 1; //Første opstart
- int tilstand = 0; //Maskine tilstand 0 = inde, 1 = ude, 2 = kører frem, 3 = kører tilbage, 10 = kalibrer1, 11 = kalibrer2, 12 = kalibrer3
- int pos = 0; //afstand der skal rejses
- bool pin_Step1_state = 0; //debounce navn
- bool pin_Step2_state = 0; //debounce navn
- bool pin_Stop_state = 0; //debounce navn
- bool pin_Kal_state = 0; //debounce navn
- bool pin_UdgPos_state = 0; //debounce navn
- int pin_Step1_count = 0; //debounce tæller
- int pin_Step2_count = 0; //debounce tæller
- int pin_Stop_count = 0; //debounce tæller
- int pin_Kal_count = 0; //debounce tæller
- int pin_UdgPos_count = 0; //debounce tæller
- bool kviter = 0; //der skal kviteres
- int currentCount = 0; //Tælleværk for kvittering
- AccelStepper stepper(AccelStepper::DRIVER, pin_STEP, pin_DIR); //Definer step motor
- //Opsætning
- void setup() {
- //Definerer INPUT/OUTPUT anvendelse
- pinMode(pin_Kvit, OUTPUT);
- pinMode(pin_Step1, INPUT);
- pinMode(pin_Step2, INPUT);
- pinMode(pin_Stop, INPUT);
- pinMode(pin_Kal, INPUT);
- pinMode(pin_UdgPos, INPUT);
- digitalWrite(pin_EN, LOW); //Sætter ENABLE aktiv
- stepper.setMaxSpeed(350); //Teoretisk max hastighed for stepper
- digitalWrite(pin_Kvit, HIGH); //sætter Kviterings udgang lav
- stepper.setAcceleration(ACCELERATE_Q_KAL);
- }
- void loop()
- {
- if(digitalRead(pin_Stop) == HIGH) {pin_Stop_count++; if(pin_Stop_count >= debounceCount) {pin_Stop_state = 1;} } else {pin_Stop_state = 0; pin_Stop_count = 0;} //debounce pin_Stop
- if(digitalRead(pin_Kal) == HIGH) {pin_Kal_count++; if(pin_Kal_count >= debounceCount) {pin_Kal_state = 1;} } else {pin_Kal_state = 0; pin_Kal_count = 0;} //debounce pin_Kal
- if(digitalRead(pin_UdgPos) == HIGH) {pin_UdgPos_count++; if(pin_UdgPos_count >= debounceCount) {pin_UdgPos_state = 1;} } else {pin_UdgPos_state = 0; pin_UdgPos_count = 0;} //debounce pin_UgdPos
- if(pin_Stop_state == 1) //Hvis der bliver kaldt på stop
- {
- nyStart = 1; //Maskinen resettes
- tilstand = 0; //sætter tilstand til 0
- }else
- {
- if((nyStart == 1) && (tilstand == 0)) //Hvis tilstand er 0 or nyStart = 1
- {
- if(pin_Kal_state == 1) //Afventer kalibrerings input fra PLC
- {
- nyStart = 0; //sætter ny start sekvens til 0
- if(pin_UdgPos_state == 1) //kontrolerer slæde position
- {
- pos = kalAfstand; //sætter pos til kalibrerings afstand
- //stepper.setAcceleration(ACCELERATE_Q_KAL); //sætter høj hastighed under 'dum' bevægelse
- //stepper.setSpeed(MAX_SPEED_KAL); //sætter hastighed for hurtig
- stepper.moveTo(pos); //kører step motor pos steps
- tilstand = 10; //sætter maskin tilstand til 10(kalibrering1)
- } else
- {
- pos = -10000; //sætter destination til et meget stort negativt tal
- //stepper.setAcceleration(ACCELERATE_Q_KAL); //sætter kalibrerings hastighed
- //stepper.setSpeed(MAX_SPEED_KAL); //Sætter hastighed for kalibrering
- stepper.moveTo(pos); //kører step motor pos steps
- tilstand = 11; //sætter maskin tilstand til 11(kalibrering2)
- }
- }
- }
- if((tilstand == 10) && (stepper.distanceToGo() == 0)) //Hvis tilstand 10 er aktiv & step motoren er færdig
- {
- tilstand = 11; //sætter maskin tilstand til 11(kalibrering2)
- pos = -10000; //sætter destination til højt negativt tal
- stepper.moveTo(pos); //definerer destination for step motor
- //stepper.setAcceleration(ACCELERATE_Q_KAL); //sætter motor accelerationen til kalibrerings hastighed
- //stepper.setSpeed(MAX_SPEED_KAL); //sætter motor max hastighed til kalibrerings hastighed
- }
- if((tilstand == 11) && (pin_UdgPos_state == 1)) //Hvis tilstand er 11 or udgangsposition er nået
- {
- stepper.stop(); //stopper motor
- stepper.setCurrentPosition(0); //sætter home position
- tilstand = 0; //sætter maskin tilstand til 0
- kviter = 1; //Anmoder om kvittering
- }
- if(nyStart != 1)
- {
- if(digitalRead(pin_Step1) == HIGH) {pin_Step1_count++; if(pin_Step1_count >= debounceCount) {pin_Step1_state = 1;} } else {pin_Step1_state = 0; pin_Step1_count = 0;} //debounce pin_Step1
- if(digitalRead(pin_Step2) == HIGH) {pin_Step2_count++; if(pin_Step2_count >= debounceCount) {pin_Step2_state = 1;} } else {pin_Step2_state = 0; pin_Step2_count = 0;} //debounce pin_Step2
- if((pin_Step1_state == HIGH) && (pin_UdgPos_state == HIGH) && (nyStart == 0) && (tilstand == 0)) //Kør frem & afvent
- {
- stepper.moveTo(Afstand); //Sætter stepper destination
- tilstand = 2; //sætter maskin tilstand til 2
- }
- if((pin_Step2_state == HIGH) && (pin_UdgPos_state == LOW) && (nyStart == 0) && (tilstand == 1)) //kør tilbage og afvent
- {
- stepper.moveTo(0); //Sætter stepper destination
- tilstand = 3; //sætter maskin tilstand til 3
- }
- }
- if((tilstand == 2) && (stepper.distanceToGo() == 0)) //Hvis tilstand = 2 og step motoren er færdig med at køre
- {
- tilstand = 1; //Sætter tilstand til 1
- kviter = 1; //Admoder om kvittering
- }
- if((tilstand == 3) && (stepper.distanceToGo() == 0)) //Hvis tilstand = 3 og step motoren er færdig med at køre
- {
- tilstand = 0; //Sætter tilstand til 0
- kviter = 1; //Anmoder om kvittering
- }
- if(kviter == 1) //Hvis der er anmodet om kvittering og den ikke allerede er sendt for længe
- {
- digitalWrite(pin_Kvit, LOW); //Aktiverer pin_Kvit relæ
- delay(200); //0.2 sek forsinkelse
- digitalWrite(pin_Kvit, HIGH); //Deaktiverer pin_Kvit relæ
- kviter = 0; //afstiller kvitering
- }
- stepper.run(); //Kør stepper hvis der er behov
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement